package com.crunch.physics;

import com.crunch.math.MathOps;
import com.crunch.math.Vector2f;

/**
 * A dynamic entity in the physics simulation.
 */
public class DynamicPhysicsEntity {
	public static final int CATEGORIES = 32;

	public static class Description {
		public Capsule shape;
		public float rotation;
		public int category;
		public Vector2f initialPosition;
		public Vector2f initialVelocity;
		public float mass;
		public boolean sticksToFloor;
		public Vector2f floorNormal;
		public float maxFloorAngle; // in radians
		public boolean sticksToFloorWhenPushed;
		public float repelAcceleration;
		public Object data;
		public EventHandler eventHandler;
	}

	public interface EventHandler {
		// called before each physics step
		void onPreStep(DynamicPhysicsEntity entity);
		// called after each physics step
		void onPostStep(DynamicPhysicsEntity entity);
		// called when input is to be processed - forces such as movement and gravity should be applied here
		void onProcessInput(DynamicPhysicsEntity entity, float dt);
	}

	DynamicPhysicsEntity(PhysicsSimulation simulation, Description description) {
		if (description.shape == null) {
			throw new IllegalArgumentException("null shape");
		}
		if (description.shape.getHeight() < 0.0f || description.shape.getRadius() <= 0.0f) {
			throw new IllegalArgumentException("Invalid shape");
		}
		if (description.category < 0 || description.category >= CATEGORIES) {
			throw new IllegalArgumentException("Invalid category");
		}
		if (description.mass < 0.0f) {
			throw new IllegalArgumentException("Negative mass");
		}
		if (description.repelAcceleration < 0.0f) {
			throw new IllegalArgumentException("Negative repel acceleration");
		}

		this.simulation = simulation;

		shape = description.shape;
		rotation = description.rotation % (2.0f * (float) Math.PI);
		if (rotation < 0.0f) {
			rotation += 2.0f * (float) Math.PI;
		}
		// upVector is (cos(rot), sin(rot)), rotated by 90 degrees CCW
		upVector = new Vector2f(-(float) Math.sin(rotation), (float) Math.cos(rotation));
		category = description.category;
		position = new Vector2f(description.initialPosition);
		velocity = new Vector2f(description.initialVelocity);
		mass = description.mass;
		sticksToFloor = description.sticksToFloor;
		floorNormal = new Vector2f(description.floorNormal);
		maxFloorAngle = description.maxFloorAngle;
		minNDotFloorNormal = Math.max(0.0f, (float) Math.sin(maxFloorAngle + 0.5f*Math.PI));
		sticksToFloorWhenPushed = description.sticksToFloorWhenPushed;
		repelAcceleration = description.repelAcceleration;
		data = description.data;
		eventHandler = description.eventHandler;
		currentRepelAcceleration = new Vector2f();

		setFlag(FLAG_IN_SIMULATION, false);
		setFlag(FLAG_EXISTS, true);

		if (simulation.simulationLock) {
			simulation.dynamicEntityAddQueue.add(this);
		} else {
			addToSimulation();
		}
	}

	void addToSimulation() {
		setFlag(FLAG_IN_SIMULATION, true);
		simulation.dynamicEntities.add(this);

		// add to dynamic grid
		Vector2f aabbMin = new Vector2f();
		Vector2f aabbMax = new Vector2f();
		getAabb(aabbMin, aabbMax, simulation.settings.contactThreshold);
		// add to hash grid - dynamic entities are activators
		gridEntity = simulation.hashGrid.addEntity(aabbMin, aabbMax, PhysicsSimulation.GRID_DYNAMIC_ENTITY_TYPE, true, this);
	}

	// removes this entity from the simulation
	public void remove() {
		checkExists();
		setFlag(FLAG_EXISTS, false);

		// remove tracked intersections
		while (trackedIntersectionList != null) {
			if (!simulation.trackedIntersectionLock) {
				PhysicsSimulation.TrackedIntersection ti = new PhysicsSimulation.TrackedIntersection();
				ti.proximityZone = trackedIntersectionList.proximityZone;
				ti.dynamicEntity = trackedIntersectionList.dynamicEntity;
				simulation.trackedIntersections.remove(ti);
			}
			PhysicsProximityZone z = trackedIntersectionList.proximityZone;
			trackedIntersectionList.unlink();

			// tracked intersection should have already been destroyed if proximity zone doesn't exist
			assert z.checkFlag(PhysicsProximityZone.FLAG_EXISTS);
			boolean validCategory = CategoryMask.doesMaskContainCategory(z.categoryMask, category);
			if (validCategory && z.eventHandler != null) {
				z.eventHandler.onExit(z, this);
			}
		}

		// remove immediately from the grid
		if (gridEntity != null) {
			gridEntity.remove();
			gridEntity = null;
		}
		// free contacts
		simulation.evaluator.contactManager.destroyContactList(contactList);
		contactList = null;
		floorContact = null;

		if (simulation.simulationLock || simulation.raycastLock) {
			// simulation is locked - we can't remove directly
			// add to deletion queue if in the simulation
			if (checkFlag(FLAG_IN_SIMULATION)) {
				simulation.dynamicEntityDeleteQueue.add(this);
			}
		} else {
			// simulation is not locked - we can directly modify without going through queues
			// if the simulation isn't locked, we should have added this entity by this point
			assert checkFlag(FLAG_IN_SIMULATION);
			// remove from simulation
			simulation.dynamicEntities.remove(this);
		}
	}

	public boolean exists() {
		return checkFlag(FLAG_EXISTS);
	}

	public void setCategory(int category) {
		checkExists();

		if (category < 0 || category >= CATEGORIES) {
			throw new IllegalArgumentException("Invalid category");
		}

		this.category = category;
	}

	public void setPosition(Vector2f position) {
		checkExists();

		if (this.position.equals(position)) {
			return; // no change
		}
		this.position.set(position);

		if (gridEntity != null) {
			// move in dynamic grid
			Vector2f aabbMin = new Vector2f();
			Vector2f aabbMax = new Vector2f();
			getAabb(aabbMin, aabbMax, simulation.settings.contactThreshold);
			// add to hash grid - dynamic entities are activators
			gridEntity.move(aabbMin, aabbMax);
		}
	}

	public void setVelocity(Vector2f velocity) {
		checkExists();
		this.velocity.set(velocity);
	}

	public void setMass(float mass) {
		checkExists();

		if (mass < 0.0f) {
			throw new IllegalArgumentException("Negative mass");
		}

		this.mass = mass;
	}

	public void setSticksToFloor(boolean sticksToFloor) {
		checkExists();
		this.sticksToFloor = sticksToFloor;
	}

	public void setFloorNormal(Vector2f floorNormal) {
		checkExists();
		this.floorNormal.set(floorNormal);
	}

	public void setMaxFloorAngle(float maxFloorAngle) {
		checkExists();
		this.maxFloorAngle = maxFloorAngle;
		minNDotFloorNormal = Math.max(0.0f, (float) Math.sin(maxFloorAngle + 0.5f*Math.PI));
	}

	public void setSticksToFloorWhenPushed(boolean sticksToFloorWhenPushed) {
		checkExists();
		this.sticksToFloorWhenPushed = sticksToFloorWhenPushed;
	}

	public void setRepelAcceleration(float repelAcceleration) {
		checkExists();

		if (repelAcceleration < 0.0f) {
			throw new IllegalArgumentException("Negative repel acceleration");
		}

		this.repelAcceleration = repelAcceleration;
	}

	public void setData(Object data) {
		checkExists();
		this.data = data;
	}

	public void setEventHandler(EventHandler eventHandler) {
		checkExists();
		this.eventHandler = eventHandler;
	}

	public PhysicsSimulation getSimulation() {
		return checkFlag(FLAG_EXISTS) ? simulation : null;
	}

	public Capsule getShape() {
		return shape;
	}

	public float getRotation() {
		return rotation;
	}

	public int getCategory() {
		return category;
	}

	public Vector2f getPosition() {
		return new Vector2f(position);
	}

	public Vector2f getVelocity() {
		return new Vector2f(velocity);
	}

	public float getMass() {
		return mass;
	}

	public boolean getSticksToFloor() {
		return sticksToFloor;
	}

	public Vector2f getFloorNormal() {
		return new Vector2f(floorNormal);
	}

	public float getRepelAcceleration() {
		return repelAcceleration;
	}

	public float getMaxFloorAngle() {
		return maxFloorAngle;
	}

	// guaranteed to be correct after simulation step
	public Contact getContactList() {
		return contactList;
	}

	// guaranteed to be correct after simulation step
	public Contact getFloorContact() {
		return floorContact;
	}

	public Object getData() {
		return data;
	}

	public EventHandler getEventHandler() {
		return eventHandler;
	}

	private PhysicsSimulation simulation;

	// fields are package-private to allow access by physics simulator
	Capsule shape;                      // shape for this entity
	float rotation;                     // rotation of this entity
	Vector2f upVector;                  // vector pointing "up" for this capsule
	int category;                       // category of this entity
	Vector2f position;                  // current position
	Vector2f velocity;                  // current velocity
	float mass;                         // entity's mass
	boolean sticksToFloor;              // whether this entity sticks to "floor" surfaces
	Vector2f floorNormal;               // normal vector which defines "floor" surfaces
	float maxFloorAngle;                // maximum angle difference from floorNormal of "floor" surfaces
	float minNDotFloorNormal;           // minimum value of n . floorNormal allow for "floor" surfaces
	boolean sticksToFloorWhenPushed;    // whether this entity sticks to "floor" surfaces when pushed
	float repelAcceleration;            // acceleration at which this entity is repelled from other entities when intersecting
	Contact contactList;                // list of contacts
	Contact floorContact;               // contact of entity and floor
	Object data;                        // user-provided data
	EventHandler eventHandler;          // event handler for this entity
	HashGrid.Entity gridEntity;         // entity in the hash grid
	Vector2f currentRepelAcceleration;  // amount of repel acceleration to be applied next step
	PhysicsSimulation.TrackedIntersectionListNode trackedIntersectionList;  // list of tracked intersection nodes

	byte flags = 0;                     // whether this entity is flagged for deletion
	static final byte FLAG_IN_SIMULATION    = 1;    // whether entity has been added to the simulation
	static final byte FLAG_EXISTS           = 2;    // whether entity "exists" to the user
	boolean checkFlag(byte flag) {
		return (flags & flag) != 0;
	}
	void setFlag(byte flag, boolean e) {
		flags = (byte) (e ? (flags | flag) : (flags & ~flag));
	}

	void checkExists() {
		if (!checkFlag(FLAG_EXISTS)) {
			throw new IllegalStateException("Not in simulation");
		}
	}

	void getAabb(Vector2f aabbMin, Vector2f aabbMax, float expansion) {
		Vector2f centerToTop = upVector.multiply(shape.getHeight()*0.5f);
		Vector2f pA = position.add(centerToTop);
		Vector2f pB = position.subtract(centerToTop);

		aabbMin.set(MathOps.min(pA, pB));
		aabbMax.set(MathOps.max(pA, pB));
		float radExp = shape.getRadius() + expansion;
		aabbMin.x(aabbMin.x() - radExp);
		aabbMin.y(aabbMin.y() - radExp);
		aabbMax.x(aabbMax.x() + radExp);
		aabbMax.y(aabbMax.y() + radExp);
	}
}
